#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
import socket
import threading


class ServerNode(Node):
    def __init__(self):
        super().__init__('server_node')
        self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10)

        # 当前指令和是否活跃标志
        self.current_cmd = None
        self.cmd_active = False

        # 高频发送定时器（50Hz）
        self.control_timer = self.create_timer(0.02, self.control_callback)  # 50Hz = 20ms

        # 启动 TCP 服务器线程
        self.server_thread = threading.Thread(target=self.run_server)
        self.server_thread.daemon = True
        self.server_thread.start()

    def run_server(self):
        self.get_logger().info('Starting TCP server...')
        server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        server_socket.bind(('0.0.0.0', 6002))
        server_socket.listen(1)
        while True:
            conn, addr = server_socket.accept()
            self.get_logger().info(f'Connected by {addr}')
            while True:
                data = conn.recv(1024)
                if not data:
                    break
                data_str = data.decode('utf-8')
                packets = data_str.split('\n')
                for packet in packets:
                    if packet.startswith('$CMD'):
                        self.process_packet(packet)

    def process_packet(self, packet):
        if not self.validate_packet(packet):
            self.get_logger().warn(f'Invalid packet: {packet}')
            return
        try:
            data_part, cs_part = packet.split('*')
            _, x_str, y_str, w_str = data_part.split(',')
            x = float(x_str)
            y = float(y_str)
            w = float(w_str)

            # 更新最新指令
            self.current_cmd = Twist()
            self.current_cmd.linear.x = x
            self.current_cmd.linear.y = y
            self.current_cmd.angular.z = w
            self.cmd_active = True

            # 重置超时计数器
            self.timeout_counter = 0
            self.get_logger().info(f'Published: x={x}, y={y}, w={w}')

        except Exception as e:
            self.get_logger().error(f'Error parsing packet: {e}')

    def control_callback(self):
        if self.cmd_active:
            # 发布当前命令
            self.publisher_.publish(self.current_cmd)
            self.timeout_counter += 1

            if self.timeout_counter >= 10:
                self.get_logger().info('Command timeout, publishing zero velocity.')
                self.publisher_.publish(Twist())
                self.cmd_active = False
        else:
            # 不活动时不发布
            pass

    def validate_packet(self, packet):
        try:
            data_part, cs_part = packet.split('*')
            cs = int(cs_part[:2], 16)
            calc_cs = 0
            for c in data_part[1:]:  # 从 C 开始计算校验和
                calc_cs ^= ord(c)
            return cs == calc_cs
        except:
            return False


def main():
    rclpy.init()
    node = ServerNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()